#include "ShooterSubsystem.h"
#include "EntechConstants.h"

ShooterSubsystem::ShooterSubsystem()
{
	m_shooterJaguar = new Jaguar(c_shooterJaguarPort);
	m_loadButton = new OperatorButton(1, c_shooterLoadButton );
	m_unloadButton = new OperatorButton(1, c_shooterUnloadButton);
	m_fireButton = new OperatorButton(1, c_shooterFireButton);
	m_manualOverride = new OperatorButton(c_shooterManualOverride);
	m_shooterProx = new DigitalInput(c_shooterProxSensor);
	m_sonar = new SonarSensor(c_sonarAnalogPort, 1);
}

ShooterSubsystem::~ShooterSubsystem()
{
}

void ShooterSubsystem::SetSubsystems(ProngsSubsystem *prongs, RollerSubsystem *roller)
{
	m_prongs = prongs;
	m_roller = roller;
}

void ShooterSubsystem::DoAutonomousInit(void)
{
	
}

void ShooterSubsystem::DoTeleopInit(void)
{
	m_shooterJaguar->Set(0);
	m_state = kFire;
}

void ShooterSubsystem::DoDisabledInit(void)
{
	m_shooterJaguar->Set(0);
}

void ShooterSubsystem::DoAutonomousPeriodic(void)
{
	
}

void ShooterSubsystem::DoTeleopPeriodic(void)
{
	if (m_manualOverride->GetBool() == true)
	{
		ManualShooter();
	}
	else
	{
		if (m_loadButton->Get() == OperatorButton::kJustPressed)
		{
			SetShooterState(kLoad);
		}
		else if (m_fireButton->Get() == OperatorButton::kJustPressed)
		{
			SetShooterState(kFire);
		}
		ShooterMotorControl();
	}
	
	if (m_shooterProx->Get() == false)
	{
		printf("shooter prox is false \n");
	}
	SmartDashboard::PutBoolean("Shooter Prox: ", !m_shooterProx->Get());
	SmartDashboard::PutNumber("Sonar distance: ", m_sonar->GetDistanceInInches());
}

void ShooterSubsystem::DoDisabledPeriodic()
{
	
}

void ShooterSubsystem::SetShooterState(ShooterState state)
{
	m_state = state;
}

void ShooterSubsystem::ShooterMotorControl()
{
	if (m_state == kLoad)
	{
		if (m_shooterProx->Get() == true && m_roller->GetIsArmDeployed() == false)
		{
			m_shooterJaguar->Set(c_shooterSpeed);
		}
		else m_shooterJaguar->Set(0);
	}
	else if (m_state == kFire)
	{
		if (m_shooterProx->Get() == false && m_fireButton->GetBool() && m_prongs->GetIsProngsDown() && m_roller->GetIsArmBack())
		{
			m_shooterJaguar->Set(c_shooterSpeed);
		}
		else m_shooterJaguar->Set(0);
	}
}

void ShooterSubsystem::ManualShooter(void)
{
	if (m_fireButton->GetBool() == true)
	{
		if (m_loadButton->GetBool() == true)
		{
			m_shooterJaguar->Set(c_shooterSpeed);
		}
		else if (m_unloadButton->GetBool() == true)
		{
			m_shooterJaguar->Set(-1 * c_shooterSpeed);
		}
		else m_shooterJaguar->Set(0);
	}
	else
	{
		m_shooterJaguar->Set(0);
	}
}
